#include "Affine.hpp"
#include <zMat.hpp>
#include <Utility/Log.hpp>
#include "../VisionTools.hpp"

namespace zzz{
  bool Affine::Create(const vector<Vector2d> &pa, const vector<Vector2d> &pb)
  {
    ZCHECK_EQ(pa.size(), pb.size());
    zuint n=pa.size();
    zMatrix<double> A(Zerosd(n*2,6));
    zVector<double> B(n*2);
    zuint cur=0;
    for (zuint i=0; i<n; i++)
    {
      const Vector2d &a=pa[i],&b=pb[i];
      A(cur,0)=a[0]; A(cur,1)=a[1]; A(cur,2)=1; B(cur)=b[0];
      cur++;
      A(cur,3)=a[0]; A(cur,4)=a[1]; A(cur,5)=1; B(cur)=b[1];
      cur++;
    }
    zMatrix<double> ATA(Trans(A)*A);
    if (!Invert(ATA))
      return false;
    zVector<double> X(ATA*Trans(A)*B);

    Hab_(0,0)=X(0); Hab_(0,1)=X(1); Hab_(0,2)=X(2);
    Hab_(1,0)=X(3); Hab_(1,1)=X(4); Hab_(1,2)=X(5);
    if (Abs(Hab_.Determinant())<EPSILON)
      return false;
    Hba_=Hab_.Inverted();
    return true;
  }
}
